#include "Subdiv.h"
#include <float.h>
#include <math.h>

const STVector3 CatClarkSurface::kUnimplementedVector(0, 0, 0);

// Load your surface from an OBJ input file.
CatClarkSurface::CatClarkSurface(const char* filename) {
  FILE* input = fopen(filename, "r");
  if (!input) {
    fprintf(stderr, "CatClarkSurface could not open OBJ %s\n", filename);
    exit(-1);
  }

  static const int kMaxLine = 1024;
  char lineBuffer[kMaxLine];
  int lineIndex = 0; // for printing error messages

  //
  // Read the file line-by-line
  //
  while (fgets(lineBuffer, kMaxLine, input)) {
    ++lineIndex;
    char* str = strtok(lineBuffer, " \t\n\r");
    //
    // Skip empty or comment lines.
    //
    if (!str || str[0] == '\0' || str[0] == '#')
      continue;
    if (str[0] == 'g' || str[0] == 'o' || str[0] == 's' || strcmp(str, "usemtl") == 0 || strcmp(str, "mtllib") == 0)
      continue;
    //
    // Process other lines based on their commands.
    //
    if (strcmp(str, "v") == 0) {
      // Vertex position line. Read the position data (x, y, z).
      str = strtok(NULL, "");
      STVector3 position;
      sscanf(str, "%f %f %f\n", &position.x, &position.y, &position.z);
      raw_vertices.push_back(position);
    } else if (strcmp(str, "vt") == 0 ||
               strcmp(str, "vn") == 0) {
      // Do nothing, we're not interested in texture/normals
    } else if (strcmp(str, "f") == 0) {
      int num_verts = 0;

      for (char* token = strtok(NULL, " \t");
           token != NULL;
           token = strtok(NULL, " \t")) {
        int index;
        int result = sscanf(token, "%d", &index);
        if (result != 1) {
          break;
        }
        num_verts++;
        // Deal with OBJ indexing rules
        if (index < 0) {
          index = raw_vertices.size() + index;
        } else {
          index -= 1;
        }
        raw_indices.push_back(index);
      }

      if (num_verts < 3) {
        fprintf(stderr, "ERROR: CatClarkSurface(%s), Line %d. Invalid Face (%d verts)\n",
                filename, lineIndex, num_verts);
        exit(-1);
      }
      raw_num_verts.push_back(num_verts);
    } else {
      fprintf(stderr, "Unable to parse line %d: %s (continuing)\n",
              lineIndex, lineBuffer);
    }
  }
  fclose(input);
}

// Save out the subdivided surface
void
CatClarkSurface::SaveOBJ(const char* filename) const {
  FILE* output = fopen(filename, "w");
  for (size_t i = 0; i < NumVertices(); i++) {
    const STVector3& vertex = GetVertex(i);
    fprintf(output, "v %f %f %f\n",
            vertex.x,
            vertex.y,
            vertex.z);
  }

  for (size_t face = 0; face < NumFaces(); face++) {
    int num_verts = NumVerticesInFace(face);
    fprintf(output, "f ");
    for (int i = 0; i < num_verts; i++) {
      fprintf(output, "%d ", 1 + GetVertexIndex(face, i));
    }
    fprintf(output, "\n");
  }
  fclose(output);
}

// Get bounding box of surface
void
CatClarkSurface::GetBounds(STVector3& boundMin, STVector3& boundMax) const {
  boundMin = STVector3( FLT_MAX,  FLT_MAX,  FLT_MAX);
  boundMax = STVector3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
  for (size_t i = 0; i < NumVertices(); i++) {
    const STVector3& v = GetVertex(i);
    for (int c = 0; c < 3; c++) {
      boundMin.Component(c) = std::min(boundMin.Component(c),
                                       v.Component(c));
      boundMax.Component(c) = std::max(boundMax.Component(c),
                                       v.Component(c));
    }
  }
}

// Subdivide the surface several times
void
CatClarkSurface::Subdivide(int iterations) {
  for (int i = 0; i < iterations; i++) {
    SubdivideOnce();
  }
}

// Draw the surface at the current level of subdivision
void
CatClarkSurface::Draw() const {
  glBegin(GL_TRIANGLES);
  for (size_t face = 0; face < NumFaces(); face++) {
    int num_verts = NumVerticesInFace(face);
    int a = GetVertexIndex(face, 0);
    int b = GetVertexIndex(face, 1);
    for (int i = 2; i < num_verts; i++) {
      int c = GetVertexIndex(face, i);
      STVector3 N = STVector3::Cross(GetVertex(b) - GetVertex(a),
                                     GetVertex(c) - GetVertex(a));
      glNormal3f(N.x, N.y, N.z);
      int indices[3] = {a, b, c};
      for (int v = 0; v < 3; v++) {
        const STVector3& posn = GetVertex(indices[v]);
        glVertex3f(posn.x, posn.y, posn.z);
		STVector3 color = posn;
		color.Normalize();
		glColor3f(color.x, color.y, color.z);
		// printf("face %i v1 %f v2 %f v3 %f \n", face, posn.x, posn.y, posn.z);
		// printf("face index a %i \n", indices[v]);
      }

      b = c;
    }
  }
  glEnd();
}

// Run your subdivision pipeline once
void
CatClarkSurface::SubdivideOnce() {
  // CS148 TODO: You'll probably want to do something like the
  // following
  BuildMesh();
  ComputeFacePoints();
  ComputeEdgePoints();
  ComputeVertexPoints();
  ConnectVertices();
}

// Build any mesh data structures you need to
void
CatClarkSurface::BuildMesh() {

	vertices.reserve(NumVertices());
	faces.reserve(NumFaces());
	edges.reserve((NumVertices()+1)/2);

	// loop all faces and access the vertices.
	for (int i = 0; i < NumFaces(); i++)
	{
		Face* face = new Face();

		for (int j =0; j < NumVerticesInFace(i); j++)
		{
			// create vertex
			STVector3 s = GetVertexOfFace(i,j);
			STVector3 e = GetVertexOfFace(i, (j+1)%NumVerticesInFace(i));
			
			Vertex* vs = findVertex(s);
			Vertex* ve = findVertex(e);

			if (vs == NULL)
			{
				vs = new Vertex(s);
				vertices.push_back(vs);
			}
			
			if (ve == NULL)
			{
				ve = new Vertex(e);
				vertices.push_back(ve);
			}
			
			// create edge
			Edge* edge = findEdge(vs->pos, ve->pos);
			if (edge == NULL)
			{
				edge = new Edge(vs, ve);
				edge->setLFace(face);
				edges.push_back(edge);

				// printf("new edge at face %i, edge %i with vs %i vj %i\n", i, j, vs, ve);
			}
			else 
			{
				edge->setRFace(face);
			}

			face->addEdge(edge);
		}

		// create face
		faces.push_back(face);
	}
}

// CS148 TODO: Implement these helper functions for your mesh.
unsigned int
CatClarkSurface::NumFaces() const {
  return raw_num_verts.size();
}

unsigned int
CatClarkSurface::NumVertices() const {	
  return raw_vertices.size();
}

unsigned int
CatClarkSurface::NumVerticesInFace(unsigned int face) const {
  return raw_num_verts[face];
}

unsigned int
CatClarkSurface::GetVertexIndex(unsigned int face, unsigned int v) const {
  int index = 0;
  for(int i = 0; i < face; i++)
    index += raw_num_verts[i];
  return raw_indices[index + v];
}

const STVector3&
CatClarkSurface::GetVertex(unsigned int i) const {
	//printf("get vertex at %i value %f %f %f \n", i, raw_vertices[i].x, raw_vertices[i].y, raw_vertices[i].z);
  return raw_vertices[i];
}

const STVector3&
CatClarkSurface::GetVertexOfFace(unsigned int face, unsigned int v) const {
  // printf("get vertex at face %i, v %i final index %i \n", face, v, GetVertexIndex(face, v));
	return GetVertex(GetVertexIndex(face, v));
}

// Compute the centroid of all faces in the mesh (face points)
void
CatClarkSurface::ComputeFacePoints() 
{
	for (int i = 0; i < faces.size(); i++)
	{
		faces[i]->computeFacePoint();		
	}
}

// Compute the Catmull Clark edge points for each edge (the average
// of the two edge endpoints and the two adjacent face points).
void
CatClarkSurface::ComputeEdgePoints() 
{
	for (int i = 0; i < edges.size(); i++)
	{
		edges[i]->computeEdgePoint();	
	}
}

// Compute the new position of each original vertex. This value is
// an averaging of the face points, the edge points and the original
// vertex.
void
CatClarkSurface::ComputeVertexPoints() 
{
	for (int i = 0; i < vertices.size(); i++)
	{
		// average face point connecting to this vertex;
		STVector3 Q(0, 0, 0);
		int num_faces = 0;

		for (int j = 0; j < faces.size(); j++)
		{
			if(faces[j]->hasVertex(vertices[i]))
			{
				Q += faces[j]->getFacePoint();
				num_faces++;
			}
		}

		Q = Q/num_faces;

		// the average of the newly computed edge points connected
		STVector3 E(0, 0, 0);
		int valence = 0;

		for (int j = 0;j < edges.size(); j++)
		{
			if(edges[j]->startV(NULL) == vertices[i] ||
				edges[j]->endV(NULL) == vertices[i])
			{
				E += edges[j]->getEdgePoint();
				valence++;
			}
		}

		E/=valence; 

		// (-Q + 4E + (n-3)*S)/n - for noboundary
		// S/2 + M/4 - for meshes with boundary

		vertices[i]->pos = (-Q + 4*E + (valence-3)*vertices[i]->pos)/valence;
	}
}

// Now that all the new positions are computed, compute the
// corresponding topology and prepare your mesh for the next round of
// subdivision/display.
void
CatClarkSurface::ConnectVertices() 
{
	raw_vertices.clear();
	raw_indices.clear();
	raw_num_verts.clear();

	raw_indices.reserve(faces.size());
	raw_vertices.reserve(vertices.size());
	raw_num_verts.reserve(faces.size()+2);

	// refresh the raw data to render. 

	for (int i=0; i < faces.size(); i++)
	{
		int edge_num = faces[i]->getNumEdge();
		for(int j = 0; j < edge_num; j++)
		{
			STVector3 sv = faces[i]->getEdge(j)->startV(faces[i])->pos;
			STVector3 ep = faces[i]->getEdge(j)->getEdgePoint();
			STVector3 fp = faces[i]->getFacePoint();
			STVector3 pre_ep = faces[i]->preEdge(j)->getEdgePoint();

			// printf("SV %i pre EV %i \n", sv, faces[i]->preEdge(j)->endV(faces[i]));

			raw_num_verts.push_back(4);
			raw_indices.push_back(insertVertex(sv));
			raw_indices.push_back(insertVertex(ep));
			raw_indices.push_back(insertVertex(fp));
			raw_indices.push_back(insertVertex(pre_ep));
		}
	}

	for (int i = 0; i < faces.size(); i++)
	{
		/*
		int num_e = faces[i]->getNumEdge();
		for (int j = 0; j < num_e ;j++)
		{
		//	printf("face %i edge %i index %i \n", i, j, GetVertexIndex(i, j), GetVertexIndex(i, (j+1)%NumVerticesInFace(i)));
		}

		STVector3 v = faces[i]->getFacePoint();
		//printf("face %i PF %f %f %f \n", i, v.x, v.y, v.z);
		*/

		delete faces[i];
	}

	for (int i = 0; i < edges.size(); i++)
	{
		/*
		int l = 0;
		int r = 0;
		for (int j = 0; j < faces.size(); j++)
		{
			if (edges[i]->getLFace() == faces[j])
				l = j;
			if (edges[i]->getRFace() == faces[j])
				r = j;
		}
		STVector3 v =  edges[i]->getEdgePoint();
		//printf("edge %i lFace %i rFace %i EP %f %f %f\n", i, l, r, v.x, v.y, v.z);
		*/

		delete edges[i];
	}

	for (int i = 0; i < vertices.size(); i++)
	{
		delete vertices[i];
		//STVector3 v =  vertices[i]->pos;
		//printf("vertex %i  VP %f %f %f \n", i, v.x, v.y, v.z);
	}

	// clear the old mesh now 
	faces.clear();
	edges.clear();
	vertices.clear();
}

// WE mesh function

Edge* CatClarkSurface::findEdge(STVector3 s, STVector3 e)
{
	for (int i=0; i < edges.size(); i++)
	{
		if (edges[i]->reverseEqual(s, e))
			return edges[i];
	}
	return NULL;
}

bool Edge::reverseEqual(STVector3 s, STVector3 e)
{
	return (vs->pos == e && ve->pos == s);
}

Vertex* CatClarkSurface::findVertex(STVector3 v)
{
	for (int i=0; i < vertices.size(); i++)
	{
		if (vertices[i]->pos == v)
			return vertices[i];
	}
	return NULL; 
}

unsigned int CatClarkSurface::insertVertex(STVector3 v)
{
	// insert if not exist, return index
	for(int i = 0; i < raw_vertices.size(); i++)
		if (raw_vertices[i] == v)
			return i;

	raw_vertices.push_back(v);
	return raw_vertices.size()-1;
}

void Face::computeFacePoint()
{
	STVector3 p(0, 0, 0);
	for(int i = 0; i < edges.size(); i++)
	{
		p += edges[i]->startV(this)->pos;
	}

	fp = p/edges.size();
}

void Edge::computeEdgePoint()
{
	STVector3 p(0, 0, 0);
	p += vs->pos;
	p += ve->pos;
	p += lFace->getFacePoint();
	p += rFace->getFacePoint();
	p /= 4;
	ep = p;
}

bool Face::hasVertex(Vertex* v)
{
	for(int i = 0; i < edges.size(); i++)
	{
		if( edges[i]->startV(this) == v)
			return true;
	}

	return false;
}

Edge* Face::preEdge(int j)
{
	if (j < 1)
		return edges[edges.size()-1];
	else
		return edges[j-1];
}

// return vs when face is null
Vertex* Edge::startV(Face* face)
{
	if (face == rFace)
		return ve;
	else 
		return vs;
}	

// return ve when face is null
Vertex* Edge::endV(Face* face)
{
	if (face == rFace)
		return vs;
	else
		return ve;
}